En esta ocasión ya tenemos un poco mas de conocimientos tanto del lenguaje de programación Python, como de la implementación de un flujo de calculo cientifico en un lenguaje de programación, pero ahora nos interesa como graficar la posición de nuestro manipulador en un plano cartesiano, por lo que empezaremos a ver la funcion:
plot()
Empezaremos por graficar una función sencilla.
$f(x) = x^2 + 1$
Primero tenemos que definirla:
In [ ]:
f = lambda x: x**2 + 1
Ahora tenemos que crear un grupo de puntos en donde evaluar nuestra función. Esto lo haremos con la función:
linspace(x0, x1, n)
la que como su nombre lo indica, creará un espacio lineal de datos desde $x_0$ hasta $x_1$, con un numero $n$ de puntos.
In [ ]:
x = linspace(-10, 10, 100)
x
Ahora que tenemos nuestro conjunto de puntos $x$ para evaluar nuestra función, podemos guardarlos en otro arreglo $y$.
In [ ]:
y = f(x)
Y podemos graficar simplemente mandando a llamar a la función de la siguiente manera:
plot(x, y)
In [ ]:
plot(x, y)
Si te fijas, tenemos una grafica de la función que queriamos, pero la función que utilizamos, tiene una particularidad que nos ayudo en este caso. El comando plot(x, y) toma como argumentos a un arreglo $x$ y otro $y$ que contienen los puntos de la grafica, pero distribuidos... tal ves será mas facil que lo veas:
In [ ]:
x
In [ ]:
y
$x$ y $y$ son dos arreglos diferentes con las coordenadas $x$ y $y$ de cada punto.
Habrá alguna manera de obtener un solo arreglo con los pares de coordenadas? La función zip().
zip(x, y)
Toma dos arreglos $x$ y $y$ y los junta en uno solo:
In [ ]:
zip(x, y)
Y ahora si, nos quedan pares de coordenadas.
En la práctica pasada obtuvimos la cinemática directa para un manipulador Stanford, sin embargo este manipulador trabaja en un espacio de 3 dimensiones, y primero tenemos que empezar con un manipulador que trabaje en 2 dimensiones, por lo que regresaremos a nuestro viejo amigo, el robot planar angular.
Primero tenemos que recordar, que la cinemática directa obtenida de este robot era:
$$ \begin{pmatrix} c_{12} & -s_{12} & 0 & l_1 c_{1} + l_2 c_{12} \\ s_{12} & c_{12} & 0 & l_1 s_{1} + l_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$Es decir, que la ubicación del efector final era:
$$ r = \begin{pmatrix} x_f \\ y_f \\ \phi_f \end{pmatrix} = \begin{pmatrix} l_1 c_1 + l_2 c_{12} \\ l_1 s_1 + l_2 s_{12} \\ q_1 + q_2 \end{pmatrix} $$Por lo que podemos obtener la posición del efector final facilmente, con las funciones $x$ y $y$
In [ ]:
xf = lambda q_1, q_2, l_1, l_2: l_1*cos(q_1) + l_2*cos(q_1 + q_2)
In [ ]:
yf = lambda q_1, q_2, l_1, l_2: l_1*sin(q_1) + l_2*sin(q_1 + q_2)
Si queremos graficar el punto final de nuestro manipulador, tan solo tenemos que mandar a llamar a la funcion plot, con estos valores.
In [ ]:
tau = 2*pi
plot([0, xf(tau/6, tau/12, 10, 15)],
[0, yf(tau/6, tau/12, 10, 15)])
Pero esto tan solo nos da la posición final ¿Que tendriamos que hacer para obtener las posiciones intermedias? La solución esta en ver las transformaciones homogeneas que habiamos hecho para este manipulador.
Recordemos un poco las transformaciones de nuestro manipulador. Existe una rotación con respecto a $z$ de $q_1$ y una traslación de $l_1$ para el primer eslabon. Para el segundo eslabon existe una rotación en $z$ de $q_2$ y una traslación de $l_2$. Lo primero que hacemos es definir las funciones para la rotación y traslación.
In [ ]:
def rotacionz(theta):
'''
Esta función devuelve una matriz de transformación homogenea para la rotación
alrededor del eje z con un angulo theta.
Uso:
>>> rotacionz(radians(30))
matrix([[ 0.8660254, -0.5 , 0. , 0. ],
[ 0.5 , 0.8660254, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
'''
Rz = matrix([[cos(theta), -sin(theta), 0, 0],
[sin(theta), cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
return Rz
def traslacionx(d):
'''
Esta función devuelve una matriz de transformación homogenea para la traslación
a lo largo del eje x con una distancia d.
Uso:
>>> traslacionx(15)
matrix([[ 1, 0, 0, 15],
[ 0, 1, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
'''
Tx = matrix([[1, 0, 0, d],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
return Tx
¿Viste las partes que aparecen en rojo? A estos se les llama comentarios de documentación, o docstrings, nos ayudan para mantener el codigo legible y entendible para cualquier persona ajena a el (o incluso para ti mismo meses despues), la proxima ves que quieras mandar a llamar a tu función intenta teclear tabulador mientras estas en el area de las variables de entrada. Asi pues, sacar la transformación homogenea del manipulador completo se puede resumir en una funcion como la que sigue:
In [ ]:
def robot_planar_angular_final(q1, q2, l1, l2):
'''
Esta función devuelve una matriz de transformación homogenea para el efector final
de un manipulador planar angular con 2 grados de libertad.
Uso:
>>> robot_planar_angular_final(radians(30), radians(50), 15, 20)
matrix([[ 0.17364818, -0.98480775, 0. , 16.46334461],
[ 0.98480775, 0.17364818, 0. , 27.19615506],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
'''
R1 = rotacionz(q1)
T2 = traslacionx(l1)
R3 = rotacionz(q2)
T4 = traslacionx(l2)
return R1*T2*R3*T4
In [ ]:
robot_planar_angular_final(tau/6, tau/12, 10, 15)
Y con esto podemos mandar llamar una matriz con los angulos que queramos y la distancia del brazo que queramos. Si esto lo multiplicamos por el vector $p_0$ que es:
$$ p_0 = \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} $$nos dará como resultado el vector de posición del efector final con respecto al origen.
In [ ]:
A = robot_planar_angular_final(tau/6, tau/12, 10, 15)
p0 = matrix([[0],
[0],
[0],
[1]])
A*p0
¿Y el punto medio? El punto medio esta justamente en la mitad de nuestras trasnformaciones, tan solo tenemos que definir otra función que nos de como resultado la matriz de transformación para el primer eslabon de nuestro manipulador.
In [ ]:
def robot_planar_angular_medio(q1, l1):
'''
Esta función devuelve una matriz de transformación homogenea para el punto medio
de un manipulador planar angular con 2 grados de libertad.
Uso:
>>> robot_planar_angular_medio(radians(30), radians(50), 15, 20)
matrix([[ -0.99690637, 0.07859831, 0. , -33.62094839],
[ -0.07859831, -0.99690637, 0. , -7.71854312],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]])
'''
R1 = rotacionz(q1)
T2 = traslacionx(l1)
return R1*T2
Y asi calcular tanto el punto medio $p_m$ como el punto final $p_f$ de nuestro manipulador.
In [ ]:
pm = robot_planar_angular_medio(tau/6, 10)*p0
pf = robot_planar_angular_final(tau/6, tau/12, 10, 15)*p0
In [ ]:
pm
In [ ]:
pf
Y notamos que las componentes en $z$ de estos puntos son $0$, lo cual es natural, ya que el manipulador que modelamos solo existe en el plano $x-y$. Tenemos que ignorar los demas datos, y dibujarlos. Revisemos la siguiente notación para las matrices.
En el pasado vimos que los arreglos no eran suficientes para poder hacer algebra lineal, y para eso esta definida la clase matrix en numpy, sin embargo no es totalmente inconexa, es decir, una matriz de numpy tambien es un arreglo, y nos podemos referir a ella de la misma manera. Si queremos el primer objeto de nuestro vector podemos acceder a el asi:
pf[0:1]
Ya que el primer elemento empieza en el lugar 0 y termina en el lugar 1. Existe una notación abreviada para lo mismo:
pf[0]
Como no queremos mas que un elemento, se puede escribir asi sin problemas, pero cuando queremos un rango es necesario poner en donde empieza y donde termina.
pf[0:2]
Nos dará los primeros dos elementos de nuestro vector.
In [ ]:
pf[0:2]
Si metemos los dos en un solo arreglo (tres de hecho, tenemos que agregar tambien el punto inicial $p_0$), podremos tenerlos listos para graficar.
In [ ]:
p = hstack([p0[0:2], pm[0:2], pf[0:2]]).tolist()
p
Esta función es una implementación de la función zip() para las matrices de numpy. Ahora tenemos 2 arreglos, uno con las componentes en $x$ y otro con las componentes en $y$.
In [ ]:
min(p[0])
In [ ]:
plot(p[0], p[1])
Y como siempre, podemos guardar todo nuestro codigo, en una función simple:
In [ ]:
def grafica_robot_planar(q1, q2, l1, l2):
'''
Esta función grafica el estado de un manipulador planar angular de dos grados
de libertad.
Uso:
>>> grafica_robot_planar(radians(30), radians(50), 15, 20)
[<matplotlib.lines.Line2D at 0xXXXXXXXXX>]
'''
p0 = matrix([[0],
[0],
[0],
[1]])
pm = robot_planar_angular_medio(q1, l1)*p0
pf = robot_planar_angular_final(q1, q2, l1, l2)*p0
p = hstack([p0[0:2], pm[0:2], pf[0:2]]).tolist()
plot(p[0], p[1])
plot(p[0], p[1], "o")
axis("scaled")
In [ ]:
grafica_robot_planar(radians(30.0), radians(50.0), 15, 20)